#include "location.h"
using namespace Eigen;
using namespace std;
using namespace cv;

Location::Location(){
    nh_ = ros::NodeHandle();
    reset_sub_ = nh_.subscribe("/detection_reset",20,&Location::detectonReset_callback,this);
};
Location::~Location(){};


void Location::detectonReset_callback(const std_msgs::Int32 msg){
    detection_reset_ = msg.data;
}


void Location::compute_dis2(vector<Eigen::VectorXf> model_circle,vector<Vector4d> &pose_laser,vector<bucket_info> &buckets_information)
{
    for(int i=0;i<pose_laser.size();i++)
    {
       if(buckets_information.size()!=0)
       {
          int count=0;
          for(int j=0;j<buckets_information.size();j++)
          {
           float distance_buc=(pose_laser[i][0]- buckets_information[j].coordinate_info.x) *(pose_laser[i][0]- buckets_information[j].coordinate_info.x)+\
           (pose_laser[i][1]- buckets_information[j].coordinate_info.y)*(pose_laser[i][1]-buckets_information[j].coordinate_info.y);
           if(distance_buc<0.16)
           {
              buckets_information[j].sum_appear.push_back(1);
              buckets_information[j].frame_bucket++;
              buckets_information[j].coordinate.push_back(Point2f(pose_laser[i][0],pose_laser[i][1]));
              count++;
           }
          }
          if(count==0)
          {
                bucket_info bucket_new;
                bucket_new.frame=1;
                bucket_new.frame_bucket=1;
                bucket_new.sum_appear.push_back(1);
                bucket_new.coordinate_info.x=pose_laser[i][0];
                bucket_new.coordinate_info.y=pose_laser[i][1];
                bucket_new.coordinate.push_back(bucket_new.coordinate_info);
                bucket_new.dis_cam_bucket = sqrt((model_circle[i][0]-752/2)*(model_circle[i][0]-752/2)+(model_circle[i][1]-600)*(model_circle[i][1]-600))/100;
                buckets_information.push_back(bucket_new);
           }
       }
       else
       {
         bucket_info bucket_new;
         bucket_new.coordinate_info.x=pose_laser[i][0];
         bucket_new.coordinate_info.y=pose_laser[i][1];
         bucket_new.coordinate.push_back(bucket_new.coordinate_info);
         bucket_new.frame=1;
         bucket_new.frame_bucket=1;
         bucket_new.sum_appear.push_back(1);
         bucket_new.dis_cam_bucket = sqrt((model_circle[i][0]-752/2)*(model_circle[i][0]-752/2)+(model_circle[i][1]-600)*(model_circle[i][1]-600))/100;
         buckets_information.push_back(bucket_new);
       }
    }
}

void Location::pose_generation(vector<Eigen::VectorXf> &model_circle,Isometry3d t1, vector<Vector4d> &pose_laser)
{
    for(int i=0;i<model_circle.size();i++)
    {
              Eigen::Vector4d trans1;
              double *tran=trans1.data();
              tran[0]=(double)fabs(model_circle[i][1]-600)/100;
              tran[1]=(double)(752/2-model_circle[i][0])/100;
              tran[2]=0;
              tran[3]=1;
              Eigen::Vector4d pose=t1*trans1;
              pose_laser.push_back(pose);
    }
}

void Location::rate_appear(Isometry3d t1,vector<bucket_info> &buckets_information,vector<Point2f> &location)
{
  for(int i=0;i<buckets_information.size();i++)
  {
    if(buckets_information[i].frame!=buckets_information[i].sum_appear.size())
    {
      buckets_information[i].sum_appear.push_back(0);
    }
  }

    for(int i=0;i<buckets_information.size();)
    {
      if(buckets_information[i].frame == 10)
      {
//        cout<<"出现帧数："<<buckets_information[i].frame_bucket<<"   总帧数："<<buckets_information[i].frame<<"    所占比率："<<(float)buckets_information[i].frame_bucket/buckets_information[i].frame<<endl;
          if((float)buckets_information[i].frame_bucket/buckets_information[i].frame<0.3)
          {
//            cout<<"清除误检测"<<endl;
            buckets_information.erase(buckets_information.begin()+i);
          }
          else if((float)buckets_information[i].frame_bucket/buckets_information[i].frame<0.6)
          {
//           cout<<"滑动窗口"<<endl;
            if(buckets_information[i].sum_appear[0]==1)
            {
              buckets_information[i].frame_bucket = buckets_information[i].frame_bucket-1;
            }
            buckets_information[i].sum_appear.erase(buckets_information[i].sum_appear.begin());
            buckets_information[i].frame = 9;
            ++i;
          }
          else
          {
            Point2f average(0.0,0.0);
            for(int j=0;j<buckets_information[i].coordinate.size();j++)
            {
              average.x += buckets_information[i].coordinate[j].x;
              average.y += buckets_information[i].coordinate[j].y;
            }
            average.x = average.x/buckets_information[i].coordinate.size();
            average.y = average.y/buckets_information[i].coordinate.size();
            location.push_back(average);

            ++i;
          }
      }
      else{
      Point2f loc_average(0.0,0.0);

//      if(buckets_information[i].frame > 10){
//        int num = 0;
//        if(buckets_information[i].sum_appear.size()>6){
//          for(int j = buckets_information[i].sum_appear.size()-1;j>=buckets_information[i].sum_appear.size()-6;--j)
//          {
//            if(buckets_information[i].sum_appear[j]==1){
//              num++;
//            }
//          }
//          if(num==0){
//            buckets_information.erase(buckets_information.begin()+i);
//          }else{
//            ++i;
//          }
//        }
//      }
      if(buckets_information[i].frame > 10&&buckets_information[i].dis_cam_bucket>2)
      {       
        if(buckets_information[i].coordinate.size()>4)
        {
          for(int j=buckets_information[i].coordinate.size()-1;j>buckets_information[i].coordinate.size()-4;j--)
          {
              loc_average.x+=buckets_information[i].coordinate[j].x;
              loc_average.y+=buckets_information[i].coordinate[j].y;
          }
          loc_average.x=loc_average.x/3;
          loc_average.y=loc_average.y/3;
          for(int j=0;j<location.size();j++)
          {
              float distance_buc=(loc_average.x- location[j].x) *(loc_average.x- location[j].x)+\
                      (loc_average.y- location[j].y)*(loc_average.y-location[j].y);
              if(distance_buc<0.16)
              {
                  location[j].x=loc_average.x;
                  location[j].y=loc_average.y;
              }
          }

          Eigen::Vector4d point_world;
          double *pose_point=point_world.data();
          pose_point[0]=loc_average.x;
          pose_point[1]=loc_average.y;
          pose_point[2]=0;
          pose_point[3]=1;

          Eigen::Vector4d pose = t1.inverse()*point_world;

          if(fabs(pose[0]/pose[1])<1.34||pose[0]<0||pose[0]>6)
          {
            buckets_information.erase(buckets_information.begin()+i);
          }
          else{
            ++i;
          }
       }
//       else if(buckets_information[i].coordinate.size()<3)
//        {
//           buckets_information.erase(buckets_information.begin()+i);
//        }
        else{
          ++i;
        }
       }
      else
        {
        ++i;
      }

     }
    }
}

void Location::follow_location(vector<Vector4d> &pose_laser1,vector<Eigen::VectorXf> &model_circle,Isometry3d t1, vector<Point2f> &location,vector<Eigen::Vector4d> &pose_array,int detection_reset)
{
    if(detection_reset_ == 1){
        frame.clear();
        buckets_information.clear();
        location.clear();
        sleep(1);
    }
    detection_reset_ = 0;
    vector<Vector4d> pose_laser;
    for(int i=0;i<buckets_information.size();i++)
    {
      buckets_information[i].frame++;
    }
      pose_generation(model_circle,t1,pose_laser);
      pose_laser1 = pose_laser;
      compute_dis2(model_circle,pose_laser,buckets_information);
      rate_appear(t1,buckets_information,location);
      for(int i=0;i<location.size();)
      {
          Eigen::Vector4d pose_world;
          double *pose_w=pose_world.data();
          pose_w[0]=location[i].x;
          pose_w[1]=location[i].y;
          pose_w[2]=0;
          pose_w[3]=1;

          Eigen::Vector4d pose_buc_cam = t1.inverse()*pose_world;
          // if(fabs(pose_buc_cam[0]/pose_buc_cam[1])>0.5&&pose_buc_cam[0]>0)
          // {
               pose_array.push_back(pose_world);
               i++;
          // }
          // else{
          //   location.erase(location.begin()+i);
          // }
      }
//      Mat Map2D_select2(1000,752, CV_32FC3,Scalar(0,0,0));
//       for(int i=0;i<location.size();i++)
//       {
//          circle(Map2D_select2, Point(location[i].x*100+600,376-100*location[i].y),25, CV_RGB(0, 255, 255), 1, 8, 0);
//       }
//      imshow("location",Map2D_select2);
//      waitKey(10);
}

void Location::areaVisualization(vector<Eigen::Vector4d> &pose_array,AreaGrid& area_visual,vector<visualization_msgs::Marker>& circles_rviz){

  //创建一个 visualization_msgs/Marker消息，并且初始化所有共享的数据。消息成员默认为0，仅仅设置位姿成员w。
  area_visual.circle.header.frame_id = "/map";
  area_visual.circle.header.stamp = ros::Time::now();
  area_visual.circle.ns = "basic_shapes";
  area_visual.circle.action = visualization_msgs::Marker::ADD;
  area_visual.circle.pose.orientation.w =  1.0;


  //分配三个不同的id到三个markers。points_and_lines名称空间的使用确保彼此不会相互冲突。
  area_visual.circle.id = 0;


  //设置marker类型到 POINTS, LINE_STRIP 和 LINE_LIST
  area_visual.circle.type = visualization_msgs::Marker::CYLINDER;


  // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
  area_visual.circle.scale.x = 0.5;
  area_visual.circle.scale.y = 0.5;
  area_visual.circle.scale.z = 1.2;

  area_visual.circle.color.r = 0.0f;
  area_visual.circle.color.g = 1.0f;
  area_visual.circle.color.b = 0.0f;
  area_visual.circle.color.a = 1;

  for(int i = 0;i<pose_array.size();i++){
  area_visual.circle.pose.position.x = pose_array[i][0];
  area_visual.circle.pose.position.y = pose_array[i][1];
  area_visual.circle.pose.position.z = 0;
  circles_rviz.push_back(area_visual.circle);
  area_visual.circle.id += 1;

}

}
